#include "visual_marker.h"

// 构造函数
VisualMarker::VisualMarker(){

}


// 析构函数
VisualMarker::~VisualMarker(){

}

// 生成颜色
std_msgs::ColorRGBA VisualMarker::GenRGBAColor(float r, float g, float b, float a){
    std_msgs::ColorRGBA color;
    color.r = r;
    color.g = g;
    color.b = b;
    color.a = a;
    return color;
}

// 生成颜色 - 蓝色
std_msgs::ColorRGBA VisualMarker::BlueColor(){
    return GenRGBAColor(0, 0, 1.0, 1);
}

// 生成颜色 - 绿色
std_msgs::ColorRGBA VisualMarker::GreenColor(){
    return GenRGBAColor(0, 1.0, 0, 1);
}

// 生成颜色 - 红色
std_msgs::ColorRGBA VisualMarker::RedColor(){
    return GenRGBAColor(1.0, 0, 0, 1);
}

// 生成颜色 - 黄色
std_msgs::ColorRGBA VisualMarker::YellowColor(){
    return GenRGBAColor(1.0, 1.0, 0, 1);
}

// 生成颜色 - 白色
std_msgs::ColorRGBA VisualMarker::WhiteColor(){
    return GenRGBAColor(1.0, 1.0, 1.0, 1);
}

// 生成颜色 - 黑色
std_msgs::ColorRGBA VisualMarker::BlackColor(){
    return GenRGBAColor(0, 0, 0, 1);
}

// 生成颜色 - 灰色
std_msgs::ColorRGBA VisualMarker::GrayColor(){
    return GenRGBAColor(0.5, 0.5, 0.5, 1);
}

// 生成尺寸
geometry_msgs::Vector3 VisualMarker::GenScale(float x, float y, float z){
    geometry_msgs::Vector3 scale;
    scale.x = x;
    scale.y = y;
    scale.z = z;
    return scale;
}

// 生成默认尺寸
geometry_msgs::Vector3 VisualMarker::DefaultScale(){
    return GenScale(0.05, 0.05, 0.05);
}


// 生成线段列表
visualization_msgs::Marker VisualMarker::GenLineList(string ns, string frame_id, std_msgs::ColorRGBA color, 
    geometry_msgs::Point start, geometry_msgs::Point end){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    marker.points.emplace_back(start);
    marker.points.emplace_back(end);

    // 高度置为0.1
    start.z = 0.1;
    end.z = 0.1;
    return marker;
}


// 生成线段列表 points需要为偶数个
visualization_msgs::Marker VisualMarker::GenLineList(string ns, string frame_id, std_msgs::ColorRGBA color, 
    vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::LINE_LIST;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    marker.points = points;

    // 高度置为0.1
    for(auto& point : marker.points){
        point.z = 0.1;
    }
    return marker;
}


// 生成圆
visualization_msgs::Marker VisualMarker::GenCircle(string ns, string frame_id, std_msgs::ColorRGBA color, 
    float radius, geometry_msgs::Point center){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    // 生成圆
    float length = 2 * M_PI * radius;
    int step = static_cast<int>(length / 0.05);
    for(int i = 0; i < step; ++i){
        float x = radius * cos(2*M_PI / step * i);
        float y = radius * sin(2*M_PI / step * i);
        geometry_msgs::Point point;
        point.x = center.x + x;
        point.y = center.y + y;
        point.z = 0.1;
        marker.points.emplace_back(point);
    }
    return marker;
}

// 生成多边形线段 points需要为偶数个
visualization_msgs::Marker VisualMarker::GenPolygonLine(string ns, string frame_id, std_msgs::ColorRGBA color, 
    vector<geometry_msgs::Point> points){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::LINE_STRIP;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    marker.points = points;
    // 添加末尾点到起点数据
    if(!points.empty()){
        marker.points.emplace_back(points.front());
    }  
    // 高度置为0.1
    for(auto& point : marker.points){
        point.z = 0.1;
    } 
    return marker;
}

// 生成圆形点集
visualization_msgs::Marker VisualMarker::GenSphere(string ns, string frame_id, std_msgs::ColorRGBA color, 
    geometry_msgs::Point center_point){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::SPHERE;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position = center_point;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    // 高度置为0.1
    center_point.z = 0.1;
    marker.points.emplace_back(center_point);
    return marker;        
}

// 生成文字
visualization_msgs::Marker VisualMarker::GenText(string ns, string frame_id, std_msgs::ColorRGBA color, 
    geometry_msgs::Point center_point, string text){
    visualization_msgs::Marker marker;
    marker.header.frame_id = frame_id;
    marker.header.stamp = ros::Time::now();
    marker.ns = ns;
    marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
    marker.action = visualization_msgs::Marker::ADD;
    marker.pose.position = center_point;
    marker.pose.orientation.w = 1.0;
    marker.scale = DefaultScale();
    marker.color = color;
    // 高度置为0.1
    center_point.z = 0.1;
    marker.points.emplace_back(center_point);
    marker.text = text;
    return marker;        
}